// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ObjectDetectionIntegratorAgentTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Threading;

    using Microsoft.Kinect;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Runtime;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Test for Vision tool interop
    /// </summary>
    [TestClass]
    public class ObjectDetectionIntegratorAgentTest
    {
        /// <summary>
        /// Validation of low level feature extraction from image
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ObjectDetectionAgentTest()
        {
            string filePath = @"\\mrevow1\f$\projects\Robots\CanDetect\tmp\ObjectDetectionAgentTest.xml";
            string kinectAgentName = "KinectAgentRaw";

            // Set up a simple pipeline to read the data file
            AgentLocator agentLocator = new AgentLocator(true);
            TimerAgent timer = new TimerAgent("FileTimer", 1);
            agentLocator.Register(timer);
            double[,] mat = new double[,] { { 584.037, 0.0, 325.914 }, { 0, 583.505, 236.022 }, { 0, 0, 1 } };

            KinectAgent kinect = new KinectAgent(
                name: kinectAgentName,
                pose: new Pose(new Vector3(-0.68384, 0.02359, 1.43758), new Quaternion(0.51747222147667016, -0.5088491918044088, 0.49156637395167707, 0.48130811337437479)),
                maximumRange: 10000,
                imageFormat: DepthImageFormat.Resolution640x480Fps30,
                rgbImageFormat: ColorImageFormat.InfraredResolution640x480Fps30,
                noReadingValue: 0,
                tooNearValue: -32002,
                tooFarValue: -32003,
                kinectMessageLag: 0,
                disableDepthAndRGBAlignment: true,
                disableRGBMirroring: false,
                intrinsicMatrix: new Matrix3(mat));
            agentLocator.Register(kinect);

            FilePlayerProxy player = new FilePlayerProxy(
                name: "player",
                filePath: filePath,
                messageTypes: new[] { typeof(KinectAgentMessage) },
                timerAgent: timer,
                binarySerialization: true);
            agentLocator.Register(player);

            KinectObjectDetectorIRAgent objectDetector = new KinectObjectDetectorIRAgent(
                            name: "objectDetector",
                            producer: player.AsProducer<KinectAgentMessage>(),
                            rawKinectAgentName: kinectAgentName,
                            receiveCountThreshold: 1,
                            detectorConfigurationFile: @"\\mrevow1\f$\projects\Robots\CanDetect\Configs\CanDetector_v1.xml");
            agentLocator.Register(objectDetector);

            int expectedMessageCount = 1;
            MessageCollectorAgent<KinectObjectDetectorResultIRMessage> collectorAgent =
                new MessageCollectorAgent<KinectObjectDetectorResultIRMessage>("collector", objectDetector, expectedMessageCount);
            IAgentHost collector = agentLocator.Register(collectorAgent);

            try
            {
                collector.Activate();

                player.Wait(5000000);
                DateTime start = DateTime.Now;

                while (collectorAgent.Results.Count <= 0 && (DateTime.Now - start).TotalMilliseconds < 500)
                {
                    Thread.Sleep(10);
                }

                if (collectorAgent.Results.Count > 0)
                {
                    KinectObjectDetectorResultIRMessage result = collectorAgent.Results.Dequeue();
                    Assert.AreEqual(result.State.Length, 2);
                    ObjectPoseEstimate 
                }
            }
            catch (Exception)
            {
                throw;
            }
            finally
            {
                collector.Deactivate();
            }
        }
    }
}
